﻿//using System;
//using System.Collections.Generic;
//using System.Diagnostics;
//using System.Linq;
//using System.Text;
//using System.Threading.Tasks;
//using System.Windows.Media;
//using Microsoft.Kinect;
//using VideoMagic.Extensions;
//using CartesianPoint = Microsoft.Kinect.SkeletonPoint;

//namespace VideoMagic
//{
//    public class KinectDepthTransform
//    {
//        const int _width = 640;
//        const int _height = 480;
//        const int _pixelCount = (_width * _height);
//        const int _bytesPerColorPixel = 4;

//        ColorImagePoint[] _pointMap = new ColorImagePoint[_pixelCount];
//        int[] _depthTransformMap = new int[_pixelCount];
//        int[] _colorTransformMap = new int[_pixelCount];
//        short _refreshDepthTransformMap = 0;

//        //short _unknownDepth;
//        //short _tooNearDepth;
//        //short _tooFarDepth;

//        KinectSensor _kinect;
//        CoordinateMapper _coordinateMapper;
//        DepthImageFormat _depthFormat;
//        ColorImageFormat _colorFormat;

//        MatrixTransform transform = new MatrixTransform();

//        float max13BitValue = (float)(Math.Pow(2, 14) - 1);

//        public KinectDepthTransform(KinectSensor kinect, DepthImageFormat depthFormat, ColorImageFormat colorFormat, int unknownDepth, int tooNearDepth, int tooFarDepth)
//        {
//            _kinect = kinect;
//            _coordinateMapper = _kinect.CoordinateMapper;
//            _depthFormat = depthFormat;
//            _colorFormat = colorFormat;
//            //_unknownDepth = (short)(unknownDepth << 3);
//            //_tooNearDepth = (short)(tooNearDepth << 3);
//            //_tooFarDepth = (short)(tooFarDepth << 3);
//        }

//        void FillTransformMap(DepthImagePixel[] depthSource)
//        {
//            _depthTransformMap.Fill(_kinect.DepthStream.TooFarDepth);
//            //_kinect.MapDepthFrameToColorFrame(_depthFormat, depthSource, _colorFormat, _pointMap);

//            _coordinateMapper.ma
//            _coordinateMapper.MapDepthFrameToColorFrame(_depthFormat, depthSource, _colorFormat, _pointMap);

//            for (int v = 0; v < _pixelCount; v++)
//            {
//                ColorImagePoint point = _pointMap[v];

//                int mappedPixel = point.Y * _width + point.X;

//                // multiply by 4 to get mapped value
//                _colorTransformMap[v] = mappedPixel << 2;

//                if (point.Y < _height && point.X < _width)
//                {
//                    _depthTransformMap[mappedPixel] = v;
//                }
//            }
//        }

//        //public Matrix GetTransformationForDepthFrame()
//        //{
//        //    short minDepth = (short)1200; // _kinect.DepthStream.MinDepth;

//        //    //short[] depthPixelData = new short[_pixelCount];
//        //    //depthPixelData.Fill(minDepth);

//        //    //ColorImagePoint[] _colorCoordinates = new ColorImagePoint[_pixelCount];
//        //    //_kinect.MapDepthFrameToColorFrame(_depthFormat, depthPixelData, _colorFormat, _colorCoordinates);

//        //    var UL = _kinect.MapDepthToColorImagePoint(_depthFormat, 0, 0, minDepth, _colorFormat);
//        //    var UR = _kinect.MapDepthToColorImagePoint(_depthFormat, _width, 0, minDepth, _colorFormat);
//        //    var LL = _kinect.MapDepthToColorImagePoint(_depthFormat, 0, _height, minDepth, _colorFormat);
//        //    var LR = _kinect.MapDepthToColorImagePoint(_depthFormat, _width, _height, minDepth, _colorFormat);


//        //    //UL = new ColorImagePoint() { X = 2, Y = 2 };
//        //    //UR = new ColorImagePoint() { X = 10, Y = 3 };
//        //    //LL = new ColorImagePoint() { X = 3, Y = 10 };
//        //    //LR = new ColorImagePoint() { X = 10, Y = 10 };

//        //    Matrix matrix = new Matrix();

//        //    matrix.OffsetX = UL.X;
//        //    matrix.OffsetY = UL.Y;

//        //    double URX = UR.X - UL.X;
//        //    double LLX = LL.X - UL.X;
//        //    double URY = UR.Y - UL.Y;
//        //    double LLY = LL.Y - UL.Y;

//        //    double offsetHeight = LR.Y - UL.Y;
//        //    double offsetWidth = LR.X - UL.X;

//        //    ////LR
//        //    //matrix.M11 * _width + matrix.M12 * _height = LR.X;
//        //    //matrix.M21 * _width + matrix.M22 * _height = LR.Y

//        //    ////UR
//        //    //matrix.M11 * _width + matrix.M12 * 0 = UR.X;
//        //    //matrix.M21 * _width + matrix.M22 * 0 = UR.Y

//        //    ////LL
//        //    //matrix.M11 * 0 + matrix.M12 * _height = LL.X;
//        //    //matrix.M21 * 0 + matrix.M22 * _height = LL.Y

//        //    matrix.M11 = .92;// URX / offsetWidth;
//        //    //matrix.M12 = LLX / offsetWidth;

//        //    //matrix.M21 = URY / offsetHeight;
//        //    matrix.M22 = .92;// LLY / offsetHeight;

//        //    return matrix;
//        //}

//        //public void TransformDepthMapForColorFrame(short[] depthSource, float[] depthTransformed)
//        //{
//        //    if (_refreshDepthTransformMap == 0)
//        //    {
//        //        // update transform map every X frames
//        //        _refreshDepthTransformMap = 5;
//        //        FillTransformMap(depthSource);
//        //    }

//        //    _refreshDepthTransformMap--;

//        //    if (depthTransformed == null)
//        //    {
//        //        depthTransformed = new float[_pixelCount];
//        //    }

//        //    Parallel.For(0, _pixelCount, v =>
//        //    {
//        //        short depthValue = (short)(depthSource[_depthTransformMap[v]] >> 3);
//        //        float depthValueFloat = (float)(((float)depthValue) / max13BitValue);

//        //        depthTransformed[v] = depthValueFloat;
//        //    });
//        //}


//        public void TransformDepthMapForColorFrame(DepthImagePixel[] depthSource, DepthImagePixel[] depthTransformed)
//        {
//            if (depthTransformed == null)
//            {
//                return;
//            }

//            DepthImagePoint[] transformed = new DepthImagePoint[_pixelCount];

//            _coordinateMapper.MapColorFrameToDepthFrame(_colorFormat, _depthFormat, depthSource, transformed);

//            //_coordinateMapper.MapDepthFrameToColorFrame(_depthFormat, depthSource, _colorFormat, depthTransformed);

//            if (_refreshDepthTransformMap == 0)
//            {
//                // update transform map every X frames
//                _refreshDepthTransformMap = 5;
//                FillTransformMap(depthSource);
//            }

//            _refreshDepthTransformMap--;

//            if (depthTransformed == null)
//            {
//                depthTransformed = new DepthImagePixel[_pixelCount];
//            }

//            Parallel.For(0, _pixelCount, v =>
//            {
//                int x = v;
//                int z = v - 10;
//                int mapValue = _depthTransformMap[x];

//                while (mapValue == -1 && x > 0 && x > z)
//                {
//                    x--;
//                    mapValue = _depthTransformMap[x];
//                }

//                if (mapValue > -1)
//                {
//                    var depthValue = (depthSource[mapValue]);

//                    if (depthValue.IsKnownDepth)
//                    {
//                        depthTransformed[v] = depthValue;
//                    }
//                }
//            });
//        }

//        //public void TransformColorToDepthFrame(byte[] colorSource, byte[] colorTransformed)
//        //{
//        //    int sourceX = 0, sourceY = 0;
//        //    short depthPixelValue = 0;

//        //    CartesianPoint x = _kinect.MapDepthToSkeletonPoint(_depthFormat, sourceX, sourceY, depthPixelValue);

//        //    //int maxBytePointer = colorSource.Length - 4;

//        //    //Parallel.For(0, _pixelCount, v =>
//        //    //{
//        //    //    int bytePointer = v << 2;
//        //    //    int mappedBytePointer = _colorTransformMap[v];

//        //    //    if (mappedBytePointer < maxBytePointer)
//        //    //    {
//        //    //        Array.Copy(colorSource, bytePointer, colorTransformed, mappedBytePointer, _bytesPerColorPixel);
//        //    //    }
//        //    //});
//        //}
//    }
//}